/*
-----------------------------------------------------------------------------
This source file is part of OGRE
(Object-oriented Graphics Rendering Engine)
For the latest info, see http://www.ogre3d.org/

Copyright (c) 2000-2011 Torus Knot Software Ltd
Copyright (c) 2006 Matthias Fink, netAllied GmbH <matthias.fink@web.de>								

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.
-----------------------------------------------------------------------------
*/
#include "OgreStableHeaders.h"
#include "OgreConvexBody.h"
#include "OgreException.h"
#include "OgreVector3.h"
#include <OgreLogManager.h>
#include <OgreRay.h>
#include <OgreFrustum.h>
#include <OgreAxisAlignedBox.h>


namespace Ogre
{


	//-----------------------------------------------------------------------
	// Statics
	//-----------------------------------------------------------------------
	ConvexBody::PolygonList ConvexBody::msFreePolygons;
#if OGRE_THREAD_SUPPORT
	OGRE_STATIC_MUTEX_INSTANCE(ConvexBody::msFreePolygonsMutex)
#endif
	//-----------------------------------------------------------------------
	void ConvexBody::_initialisePool()
	{
		OGRE_LOCK_MUTEX(msFreePolygonsMutex)

		if (msFreePolygons.empty())
		{
			const size_t initialSize = 30;

			// Initialise polygon pool with 30 polys
			msFreePolygons.resize(initialSize);
			for (size_t i = 0; i < initialSize; ++i)
			{
				msFreePolygons[i] = OGRE_NEW_T(Polygon, MEMCATEGORY_SCENE_CONTROL)();
			}
		}
	}
	//-----------------------------------------------------------------------
	void ConvexBody::_destroyPool()
	{
		OGRE_LOCK_MUTEX(msFreePolygonsMutex)
		
		for (PolygonList::iterator i = msFreePolygons.begin(); 
			i != msFreePolygons.end(); ++i)
		{
			OGRE_DELETE_T(*i, Polygon, MEMCATEGORY_SCENE_CONTROL);
		}
		msFreePolygons.clear();
	}
	//-----------------------------------------------------------------------
	Polygon* ConvexBody::allocatePolygon()
	{
		OGRE_LOCK_MUTEX(msFreePolygonsMutex)

		if (msFreePolygons.empty())
		{
			// if we ran out of polys to use, create a new one
			// hopefully this one will return to the pool in due course
			return OGRE_NEW_T(Polygon, MEMCATEGORY_SCENE_CONTROL)();
		}
		else
		{
			Polygon* ret = msFreePolygons.back();
			ret->reset();

			msFreePolygons.pop_back();

			return ret;

		}
	}
	//-----------------------------------------------------------------------
	void ConvexBody::freePolygon(Polygon* poly)
	{
		OGRE_LOCK_MUTEX(msFreePolygonsMutex)
		msFreePolygons.push_back(poly);
	}
	//-----------------------------------------------------------------------
	//-----------------------------------------------------------------------
	ConvexBody::ConvexBody()
	{
		// Reserve space for 8 polys, normally 6 faces plus a couple of clips
		mPolygons.reserve(8);

	}
	//-----------------------------------------------------------------------
	ConvexBody::~ConvexBody()
	{
		reset();
	}
	//-----------------------------------------------------------------------
	ConvexBody::ConvexBody( const ConvexBody& cpy )
	{
		for ( size_t i = 0; i < cpy.getPolygonCount(); ++i )
		{
			Polygon *p = allocatePolygon();
			*p = cpy.getPolygon( i );
			mPolygons.push_back( p );
		}
	}
	//-----------------------------------------------------------------------
	void ConvexBody::define(const Frustum& frustum)
	{
		// ordering of the points:
		// near (0-3), far (4-7); each (top-right, top-left, bottom-left, bottom-right)
		//	   5-----4
		//	  /|    /|
		//	 / |   / |
		//	1-----0  |
		//	|  6--|--7
		//	| /   | /
		//	|/    |/
		//	2-----3
		
		const Vector3 *pts = frustum.getWorldSpaceCorners();

		/// reset ConvexBody
		reset();

		/// update vertices: near, far, left, right, bottom, top; fill in ccw
		Polygon *poly;

		// near
		poly = allocatePolygon();
		poly->insertVertex( pts[0] );
		poly->insertVertex( pts[1] );
		poly->insertVertex( pts[2] );
		poly->insertVertex( pts[3] );
		mPolygons.push_back( poly );

		// far
		poly = allocatePolygon();
		poly->insertVertex( pts[5] );
		poly->insertVertex( pts[4] );
		poly->insertVertex( pts[7] );
		poly->insertVertex( pts[6] );
		mPolygons.push_back( poly );

		// left
		poly = allocatePolygon();
		poly->insertVertex( pts[5] );
		poly->insertVertex( pts[6] );
		poly->insertVertex( pts[2] );
		poly->insertVertex( pts[1] );
		mPolygons.push_back( poly ); 

		// right
		poly = allocatePolygon();
		poly->insertVertex( pts[4] );
		poly->insertVertex( pts[0] );
		poly->insertVertex( pts[3] );
		poly->insertVertex( pts[7] );
		mPolygons.push_back( poly ); 

		// bottom
		poly = allocatePolygon();
		poly->insertVertex( pts[6] );
		poly->insertVertex( pts[7] );
		poly->insertVertex( pts[3] );
		poly->insertVertex( pts[2] );
		mPolygons.push_back( poly ); 

		// top
		poly = allocatePolygon();
		poly->insertVertex( pts[4] );
		poly->insertVertex( pts[5] );
		poly->insertVertex( pts[1] );
		poly->insertVertex( pts[0] );
		mPolygons.push_back( poly ); 
	}
	//-----------------------------------------------------------------------
	void ConvexBody::define(const AxisAlignedBox& aab)
	{
		// ordering of the AAB points:
		//		1-----2
		//	   /|    /|
		//	  / |   / |
		//   5-----4  |
		//   |  0--|--3
		//   | /   | /
		//   |/    |/
		//   6-----7
		
		const Vector3& min = aab.getMinimum();
		const Vector3& max = aab.getMaximum();

		Vector3 currentVertex = min;

		Polygon *poly;

		// reset body
		reset();

		// far
		poly = allocatePolygon();
		poly->insertVertex( currentVertex ); // 0 
		currentVertex.y = max.y;
		poly->insertVertex( currentVertex ); // 1
		currentVertex.x = max.x;
		poly->insertVertex( currentVertex ); // 2
		currentVertex.y = min.y;
		poly->insertVertex( currentVertex ); // 3
		insertPolygon( poly );

		// right
		poly = allocatePolygon();
		poly->insertVertex( currentVertex ); // 3
		currentVertex.y = max.y;
		poly->insertVertex( currentVertex ); // 2
		currentVertex.z = max.z;
		poly->insertVertex( currentVertex ); // 4
		currentVertex.y = min.y;
		poly->insertVertex( currentVertex ); // 7
		insertPolygon( poly ); 

		// near
		poly = allocatePolygon();
		poly->insertVertex( currentVertex ); // 7
		currentVertex.y = max.y;
		poly->insertVertex( currentVertex ); // 4
		currentVertex.x = min.x;
		poly->insertVertex( currentVertex ); // 5
		currentVertex.y = min.y;
		poly->insertVertex( currentVertex ); // 6
		insertPolygon( poly );

		// left
		poly = allocatePolygon();
		poly->insertVertex( currentVertex ); // 6
		currentVertex.y = max.y;
		poly->insertVertex( currentVertex ); // 5
		currentVertex.z = min.z;
		poly->insertVertex( currentVertex ); // 1
		currentVertex.y = min.y;
		poly->insertVertex( currentVertex ); // 0
		insertPolygon( poly ); 

		// bottom
		poly = allocatePolygon();
		poly->insertVertex( currentVertex ); // 0 
		currentVertex.x = max.x;
		poly->insertVertex( currentVertex ); // 3
		currentVertex.z = max.z;
		poly->insertVertex( currentVertex ); // 7 
		currentVertex.x = min.x;
		poly->insertVertex( currentVertex ); // 6
		insertPolygon( poly );

		// top
		poly = allocatePolygon();
		currentVertex = max;
		poly->insertVertex( currentVertex ); // 4
		currentVertex.z = min.z;
		poly->insertVertex( currentVertex ); // 2
		currentVertex.x = min.x;
		poly->insertVertex( currentVertex ); // 1
		currentVertex.z = max.z;
		poly->insertVertex( currentVertex ); // 5
		insertPolygon( poly );
		
	}
	//-----------------------------------------------------------------------
	void ConvexBody::clip(const AxisAlignedBox& aab)
	{
		// only process finite boxes
		if (!aab.isFinite())
			return;
		// ordering of the AAB points:
		//		1-----2
		//	   /|    /|
		//	  / |   / |
		//   5-----4  |
		//   |  0--|--3
		//   | /   | /
		//   |/    |/
		//   6-----7

		const Vector3& min = aab.getMinimum();
		const Vector3& max = aab.getMaximum();

		// clip object for each plane of the AAB
		Plane p;


		// front
		p.redefine(Vector3::UNIT_Z, max);
		clip(p);

		// back
		p.redefine(Vector3::NEGATIVE_UNIT_Z, min);
		clip(p);
		
		// left
		p.redefine(Vector3::NEGATIVE_UNIT_X, min);
		clip(p);
		
		// right
		p.redefine(Vector3::UNIT_X, max);
		clip(p);
		
		// bottom
		p.redefine(Vector3::NEGATIVE_UNIT_Y, min);
		clip(p);
		
		// top
		p.redefine(Vector3::UNIT_Y, max);
		clip(p);

	}
	//-----------------------------------------------------------------------
	void ConvexBody::clip(const Frustum& fr)
	{
		// clip the body with each plane
		for ( unsigned short i = 0; i < 6; ++i )
		{
			// clip, but keep positive space this time since frustum planes are 
			// the opposite to other cases (facing inwards rather than outwards)
			clip(fr.getFrustumPlane(i), false);
		}
	}
	//-----------------------------------------------------------------------
	void ConvexBody::clip(const ConvexBody& body)
	{
		if ( this == &body )
			return;

		// for each polygon; clip 'this' with each plane of 'body'
		// front vertex representation is ccw

		Plane pl;

		for ( size_t iPoly = 0; iPoly < body.getPolygonCount(); ++iPoly )
		{
			const Polygon& p = body.getPolygon( iPoly );

			OgreAssert( p.getVertexCount() >= 3, "A valid polygon must contain at least three vertices." );

			// set up plane with first three vertices of the polygon (a polygon is always planar)
			pl.redefine( p.getVertex( 0 ), p.getVertex( 1 ), p.getVertex( 2 ) );

			clip(pl);
		}
	}
	//-----------------------------------------------------------------------
	void ConvexBody::extend(const Vector3& pt)
	{
		// Erase all polygons facing towards the point. For all edges that
		// are not removed twice (once in AB and once BA direction) build a
		// convex polygon (triangle) with the point.
		Polygon::EdgeMap edgeMap;

		for ( size_t i = 0; i < getPolygonCount(); ++i )
		{
			const Vector3& normal = getNormal( i );
			// direction of the point in regard to the polygon
			// the polygon is planar so we can take an arbitrary vertex
			Vector3 ptDir  = pt - getVertex( i, 0 );
			ptDir.normalise();

			// remove polygon if dot product is greater or equals null.
			if ( normal.dotProduct( ptDir ) >= 0 )
			{
				// store edges (copy them because if the polygon is deleted
				// its vertices are also deleted)
				storeEdgesOfPolygon( i, &edgeMap );

				// remove polygon
				deletePolygon( i );

				// decrement iterator because of deleted polygon
				--i; 
			}
		}

		// point is already a part of the hull (point lies inside)
		if ( edgeMap.empty() )
			return;

		// remove the edges that are twice in the list (once from each side: AB,BA)

		Polygon::EdgeMap::iterator it;
		// iterate from first to the element before the last one
		for (Polygon::EdgeMap::iterator itStart = edgeMap.begin(); 
			itStart != edgeMap.end(); )
		{
			// compare with iterator + 1 to end
			// don't need to skip last entry in itStart since omitted in inner loop
			it = itStart;
			++it;

			bool erased = false;
			// iterate from itStart+1 to the element before the last one
			for ( ; it != edgeMap.end(); ++it )
			{	
				if (itStart->first.positionEquals(it->second) &&
					 itStart->second.positionEquals(it->first))
				{
					edgeMap.erase(it);
					// increment itStart before deletion (iterator invalidation)
					Polygon::EdgeMap::iterator delistart = itStart++;
					edgeMap.erase(delistart);
					erased = true;

					break; // found and erased
				}
			}
			// increment itStart if we didn't do it when erasing
			if (!erased)
				++itStart;

		}

		// use the remaining edges to build triangles with the point
		// the vertices of the edges are in ccw order (edgePtA-edgePtB-point
		// to form a ccw polygon)
		while ( !edgeMap.empty() )
		{
			Polygon::EdgeMap::iterator mapIt = edgeMap.begin();

			// build polygon it.first, it.second, point
			Polygon *p = allocatePolygon();

			p->insertVertex(mapIt->first);
			p->insertVertex(mapIt->second);

			p->insertVertex( pt );
			// attach polygon to body
			insertPolygon( p );

			// erase the vertices from the list
			// pointers are now held by the polygon
			edgeMap.erase( mapIt );
		}
	}
	//-----------------------------------------------------------------------
	void ConvexBody::reset( void )
	{
		for (PolygonList::iterator it = mPolygons.begin(); 
			it != mPolygons.end(); ++it)
		{
			freePolygon(*it);
		}
		mPolygons.clear();
	}
	//-----------------------------------------------------------------------
	size_t ConvexBody::getPolygonCount( void ) const
	{
		return mPolygons.size();
	}
	//-----------------------------------------------------------------------
	size_t ConvexBody::getVertexCount( size_t poly ) const
	{
		OgreAssert(poly < getPolygonCount(), "Search position out of range" );
		
		return mPolygons[ poly ]->getVertexCount();
	}
	//-----------------------------------------------------------------------
	bool ConvexBody::hasClosedHull( void ) const
	{
		// if this map is returned empty, the body is closed
		Polygon::EdgeMap edgeMap = getSingleEdges();

		return edgeMap.empty();
	}
	//-----------------------------------------------------------------------
	void ConvexBody::mergePolygons( void )
	{
		// Merge all polygons that lay in the same plane as one big polygon.
		// A convex body does not have two seperate regions (seperated by polygons
		// with different normals) where the same normal occurs, so we can simply
		// search all similar normals of a polygon. Two different options are 
		// possible when the normals fit:
		// - the two polygons are neighbors
		// - the two polygons aren't neighbors (but a third, fourth,.. polygon lays
		//   in between)

		// Signals if the body holds polygons which aren't neighbors but have the same
		// normal. That means another step has to be processed.
		bool bDirty = false;

		for ( size_t iPolyA = 0; iPolyA < getPolygonCount(); ++iPolyA )
		{
			// ??
			OgreAssert( iPolyA >= 0, "strange..." );

			for ( size_t iPolyB = iPolyA+1; iPolyB < getPolygonCount(); ++iPolyB )
			{
				const Vector3& n1 = getNormal( iPolyA );
				const Vector3& n2 = getNormal( iPolyB );

				// if the normals point into the same direction
				if ( n1.directionEquals( n2, Radian( Degree( 0.00001 ) ) )  )
				{
					// indicates if a neighbor has been found and joined
					bool bFound = false;

					// search the two fitting vertices (if there are any) for the common edge
					const size_t numVerticesA = getVertexCount( iPolyA );
					for ( size_t iVertexA = 0; iVertexA < numVerticesA; ++iVertexA )
					{
						const size_t numVerticesB = getVertexCount( iPolyB );
						for ( size_t iVertexB = 0; iVertexB < numVerticesB; ++iVertexB )
						{
							const Vector3& aCurrent	= getVertex( iPolyA, iVertexA );
							const Vector3& aNext		= getVertex( iPolyA, (iVertexA + 1) % getVertexCount( iPolyA ) );
							const Vector3& bCurrent	= getVertex( iPolyB, iVertexB );
							const Vector3& bNext		= getVertex( iPolyB, (iVertexB + 1) % getVertexCount( iPolyB ) );

							// if the edge is the same the current vertex of A has to be equal to the next of B and the other
							// way round
							if ( aCurrent.positionEquals(bNext) &&
								 bCurrent.positionEquals(aNext))
							{
								// polygons are neighbors, assemble new one
								Polygon *pNew = allocatePolygon();

								// insert all vertices of A up to the join (including the common vertex, ignoring
								// whether the first vertex of A may be a shared vertex)
								for ( size_t i = 0; i <= iVertexA; ++i )
								{
									pNew->insertVertex( getVertex( iPolyA, i%numVerticesA ) );
								}

								// insert all vertices of B _after_ the join to the end
								for ( size_t i = iVertexB + 2; i < numVerticesB; ++i )
								{
									pNew->insertVertex( getVertex( iPolyB, i ) );
								}

								// insert all vertices of B from the beginning up to the join (including the common vertex
								// and excluding the first vertex if the first is part of the shared edge)
								for ( size_t i = 0; i <= iVertexB; ++i )
								{
									pNew->insertVertex( getVertex( iPolyB, i%numVerticesB ) );
								}

								// insert all vertices of A _after_ the join to the end
								for ( size_t i = iVertexA + 2; i < numVerticesA; ++i )
								{
									pNew->insertVertex( getVertex( iPolyA, i ) );
								}

								// in case there are double vertices (in special cases), remove them
								for ( size_t i = 0; i < pNew->getVertexCount(); ++i )
								{
									const Vector3& a = pNew->getVertex( i );
									const Vector3& b = pNew->getVertex( (i + 1) % pNew->getVertexCount() );

									// if the two vertices are the same...
									if (a.positionEquals(b))
									{
										// remove a
										pNew->deleteVertex( i );

										// decrement counter
										--i;
									}
								}

								// delete the two old ones
								OgreAssert( iPolyA != iPolyB, "PolyA and polyB are the same!" );
								
								// polyB is always higher than polyA, so delete polyB first
								deletePolygon( iPolyB );
								deletePolygon( iPolyA );

								// continue with next (current is deleted, so don't jump to the next after the next)
								--iPolyA;
								--iPolyB;

								// insert new polygon
								insertPolygon( pNew );

								bFound = true;
								break;
							}
						}
						
						if ( bFound )
						{
							break;
						}
					}

					if ( bFound == false )
					{
						// there are two polygons available with the same normal direction, but they
						// could not be merged into one single because of no shared edge
						bDirty = true;
						break;
					}
				}
			}
		}

		// recursion to merge the previous non-neighbors
		if ( bDirty )
		{
			mergePolygons();
		}
	}
	//-----------------------------------------------------------------------
	const Vector3& ConvexBody::getNormal( size_t poly )
	{
		OgreAssert( poly >= 0 && poly < getPolygonCount(), "Search position out of range" );
		
		return mPolygons[ poly ]->getNormal();
	}
	//-----------------------------------------------------------------------
	AxisAlignedBox ConvexBody::getAABB( void ) const
	{
		AxisAlignedBox aab;

		for ( size_t i = 0; i < getPolygonCount(); ++i )
		{
			for ( size_t j = 0; j < getVertexCount( i ); ++j )
			{
				aab.merge( getVertex( i, j ) );
			}
		}

		return aab;
	}
	//-----------------------------------------------------------------------
	bool ConvexBody::operator == ( const ConvexBody& rhs ) const
	{
		if ( getPolygonCount() != rhs.getPolygonCount() )
			return false;

		// Compare the polygons. They may not be in correct order.
		// A correct convex body does not have identical polygons in its body.
		bool *bChecked = OGRE_ALLOC_T(bool, getPolygonCount(), MEMCATEGORY_SCENE_CONTROL);
		for ( size_t i=0; i<getPolygonCount(); ++i )
		{
			bChecked[ i ] = false;
		}

		for ( size_t i=0; i<getPolygonCount(); ++i )
		{
			bool bFound = false;

			for ( size_t j=0; j<getPolygonCount(); ++j )
			{
				const Polygon& pA = getPolygon( i );
				const Polygon& pB = rhs.getPolygon( j );

				if ( pA == pB )
				{
					bFound = true;
					bChecked[ i ] = true;
					break;
				}
			}

			if ( bFound == false )
			{
				OGRE_FREE(bChecked, MEMCATEGORY_SCENE_CONTROL);
				bChecked = 0;
				return false;
			}
		}

		for ( size_t i=0; i<getPolygonCount(); ++i )
		{
			if ( bChecked[ i ] != true )
			{
				OGRE_FREE(bChecked, MEMCATEGORY_SCENE_CONTROL);
				bChecked = 0;
				return false;
			}
		}

		OGRE_FREE(bChecked, MEMCATEGORY_SCENE_CONTROL);
		bChecked = 0;
		return true;
	}
	//-----------------------------------------------------------------------
	std::ostream& operator<< ( std::ostream& strm, const ConvexBody& body )
	{
		strm << "POLYGON INFO (" << body.getPolygonCount() << ")" << std::endl;

		for ( size_t i = 0; i < body.getPolygonCount(); ++i )
		{
			strm << "POLYGON " << i << ", ";
			strm << body.getPolygon( i );
		}

		return strm;
	}
	//-----------------------------------------------------------------------
	void ConvexBody::insertPolygon(Polygon* pdata, size_t poly )
	{
		OgreAssert(poly <= getPolygonCount(), "Insert position out of range" );
		OgreAssert( pdata != NULL, "Polygon is NULL" );

		PolygonList::iterator it = mPolygons.begin();
		std::advance(it, poly);

		mPolygons.insert( it, pdata );

	}
	//-----------------------------------------------------------------------
	void ConvexBody::insertPolygon(Polygon* pdata)
	{
		OgreAssert( pdata != NULL, "Polygon is NULL" );

		mPolygons.push_back( pdata );

	}
	//-----------------------------------------------------------------------
	void ConvexBody::insertVertex(size_t poly, const Vector3& vdata, size_t vertex )
	{
		OgreAssert(poly < getPolygonCount(), "Search position (polygon) out of range" );
		
		mPolygons[poly]->insertVertex(vdata, vertex);
	}
	//-----------------------------------------------------------------------
	void ConvexBody::insertVertex(size_t poly, const Vector3& vdata)
	{
		OgreAssert(poly < getPolygonCount(), "Search position (polygon) out of range" );

		mPolygons[poly]->insertVertex(vdata);
	}
	//-----------------------------------------------------------------------
	void ConvexBody::deletePolygon(size_t poly)
	{
		OgreAssert(poly < getPolygonCount(), "Search position out of range" );

		PolygonList::iterator it = mPolygons.begin();
		std::advance(it, poly);
		
		freePolygon(*it);
		mPolygons.erase(it);
	}
	//-----------------------------------------------------------------------
	Polygon* ConvexBody::unlinkPolygon(size_t poly)
	{
		OgreAssert( poly >= 0 && poly < getPolygonCount(), "Search position out of range" );

		PolygonList::iterator it = mPolygons.begin();
		std::advance(it, poly);

		// safe address
		Polygon *pRet = *it;
		
		// delete entry
		mPolygons.erase(it);	

		// return polygon pointer

		return pRet;
	}
	//-----------------------------------------------------------------------
	void ConvexBody::moveDataFromBody(ConvexBody& body)
	{
		body.mPolygons.swap(this->mPolygons);
	}
	//-----------------------------------------------------------------------
	void ConvexBody::deleteVertex(size_t poly, size_t vertex)
	{
		OgreAssert(poly < getPolygonCount(), "Search position out of range" );

		mPolygons[poly]->deleteVertex(vertex);
	}
	//-----------------------------------------------------------------------
	const Polygon& ConvexBody::getPolygon(size_t poly) const
	{
		OgreAssert(poly < getPolygonCount(), "Search position out of range");

		return *mPolygons[poly];
	}
	//-----------------------------------------------------------------------
	void ConvexBody::setPolygon(Polygon* pdata, size_t poly)
	{
		OgreAssert(poly < getPolygonCount(), "Search position out of range" );
		OgreAssert(pdata != NULL, "Polygon is NULL" );

		if (pdata != mPolygons[poly])
		{
			// delete old polygon
			freePolygon(mPolygons[ poly ]);

			// set new polygon
			mPolygons[poly] = pdata;
		}
	}
	//-----------------------------------------------------------------------
	const Vector3& ConvexBody::getVertex(size_t poly, size_t vertex) const
	{
		OgreAssert( poly >= 0 && poly < getPolygonCount(), "Search position out of range" );
		
		return mPolygons[poly]->getVertex(vertex);
	}
	//-----------------------------------------------------------------------
	void ConvexBody::setVertex(size_t poly, const Vector3& vdata, size_t vertex)
	{
		OgreAssert(poly < getPolygonCount(), "Search position out of range");
		
		mPolygons[poly]->setVertex(vdata, vertex);
	}
	//-----------------------------------------------------------------------
	void ConvexBody::storeEdgesOfPolygon(size_t poly, Polygon::EdgeMap *edgeMap ) const
	{
		OgreAssert(poly <= getPolygonCount(), "Search position out of range" );
		OgreAssert( edgeMap != NULL, "TEdgeMap ptr is NULL" );

		mPolygons[poly]->storeEdges(edgeMap);
	}
	//-----------------------------------------------------------------------
	Polygon::EdgeMap ConvexBody::getSingleEdges() const
	{
		Polygon::EdgeMap edgeMap;

		// put all edges of all polygons into a list every edge has to be
		// walked in each direction once	
		for ( size_t i = 0; i < getPolygonCount(); ++i )
		{
			const Polygon& p = getPolygon( i );

			for ( size_t j = 0; j < p.getVertexCount(); ++j )
			{
				const Vector3& a = p.getVertex( j );
				const Vector3& b = p.getVertex( ( j + 1 ) % p.getVertexCount() );

				edgeMap.insert( Polygon::Edge( a, b ) );
			}
		}

		// search corresponding parts
		Polygon::EdgeMap::iterator it;
		Polygon::EdgeMap::iterator itStart;
		Polygon::EdgeMap::const_iterator itEnd;
		while( !edgeMap.empty() )
		{
			it = edgeMap.begin(); ++it;	// start one element after itStart
			itStart = edgeMap.begin();	// the element to be compared with the others
			itEnd = edgeMap.end();		// beyond the last element
			
			bool bFound = false;

			for ( ; it != itEnd; ++it )
			{
				if (itStart->first.positionEquals(it->second) &&
					 itStart->second.positionEquals(it->first))
				{
					// erase itStart and it
					edgeMap.erase( it );
					edgeMap.erase( itStart );

					bFound = true;

					break; // found
				}
			}

			if ( bFound == false )
			{
				break;	// not all edges could be matched
						// body is not closed
			}
		}

		return edgeMap;
	}
	//-----------------------------------------------------------------------
	void ConvexBody::allocateSpace( size_t numPolygons, size_t numVertices )
	{
		reset();

		// allocate numPolygons polygons with each numVertices vertices
		for ( size_t iPoly = 0; iPoly < numPolygons; ++iPoly )
		{
			Polygon *poly = allocatePolygon();

			for ( size_t iVertex = 0; iVertex < numVertices; ++iVertex )
			{
				poly->insertVertex( Vector3::ZERO );
			}

			mPolygons.push_back( poly );
		}
	}
	//-----------------------------------------------------------------------
	void ConvexBody::clip( const Plane& pl, bool keepNegative )
	{
		if ( getPolygonCount() == 0 )
			return;

		// current will be used as the reference body
		ConvexBody current;
		current.moveDataFromBody(*this);
		
		OgreAssert( this->getPolygonCount() == 0, "Body not empty!" );
		OgreAssert( current.getPolygonCount() != 0, "Body empty!" );

		// holds all intersection edges for the different polygons
		Polygon::EdgeMap intersectionEdges;

		// clip all polygons by the intersection plane
		// add only valid or intersected polygons to *this
		for ( size_t iPoly = 0; iPoly < current.getPolygonCount(); ++iPoly )
		{

			// fetch vertex count and ignore polygons with less than three vertices
			// the polygon is not valid and won't be added
			const size_t vertexCount = current.getVertexCount( iPoly );
			if ( vertexCount < 3 )
				continue;

			// current polygon
			const Polygon& p = current.getPolygon( iPoly );

			// the polygon to assemble
			Polygon *pNew = allocatePolygon();

			// the intersection polygon (indeed it's an edge or it's empty)
			Polygon *pIntersect = allocatePolygon();
			
			// check if polygons lie inside or outside (or on the plane)
			// for each vertex check where it is situated in regard to the plane
			// three possibilities appear:
			Plane::Side clipSide = keepNegative ? Plane::POSITIVE_SIDE : Plane::NEGATIVE_SIDE;
			// - side is clipSide: vertex will be clipped
			// - side is !clipSide: vertex will be untouched
			// - side is NOSIDE:   vertex will be untouched
			Plane::Side *side = OGRE_ALLOC_T(Plane::Side, vertexCount, MEMCATEGORY_SCENE_CONTROL);
			for ( size_t iVertex = 0; iVertex < vertexCount; ++iVertex )
			{
				side[ iVertex ] = pl.getSide( p.getVertex( iVertex ) );
			}

			// now we check the side combinations for the current and the next vertex
			// four different combinations exist:
			// - both points inside (or on the plane): keep the second (add it to the body)
			// - both points outside: discard both (don't add them to the body)
			// - first vertex is inside, second is outside: add the intersection point
			// - first vertex is outside, second is inside: add the intersection point, then the second
			for ( size_t iVertex = 0; iVertex < vertexCount; ++iVertex )
			{
				// determine the next vertex
				size_t iNextVertex = ( iVertex + 1 ) % vertexCount;

				const Vector3& vCurrent = p.getVertex( iVertex );
				const Vector3& vNext    = p.getVertex( iNextVertex );

				// case 1: both points inside (store next)
				if ( side[ iVertex ]     != clipSide &&		// NEGATIVE or NONE
					 side[ iNextVertex ] != clipSide )		// NEGATIVE or NONE
				{
					// keep the second
					pNew->insertVertex( vNext );
				}

				// case 3: inside -> outside (store intersection)
				else if ( side[ iVertex ]		!= clipSide &&
						  side[ iNextVertex ]	== clipSide )
				{
					// Do an intersection with the plane. We use a ray with a start point and a direction.
					// The ray is forced to hit the plane with any option available (eigher current or next
					// is the starting point)

					// intersect from the outside vertex towards the inside one
					Vector3 vDirection = vCurrent - vNext;
					vDirection.normalise();
					Ray ray( vNext, vDirection );
					std::pair< bool, Real > intersect = ray.intersects( pl );

					// store intersection
					if ( intersect.first )
					{
						// convert distance to vector
						Vector3 vIntersect = ray.getPoint( intersect.second );	

						// store intersection
						pNew->insertVertex( vIntersect );
						pIntersect->insertVertex( vIntersect );
					}
				}

				// case 4: outside -> inside (store intersection, store next)
				else if ( side[ iVertex ]		== clipSide &&
					side[ iNextVertex ]			!= clipSide )
				{
					// Do an intersection with the plane. We use a ray with a start point and a direction.
					// The ray is forced to hit the plane with any option available (eigher current or next
					// is the starting point)

					// intersect from the outside vertex towards the inside one
					Vector3 vDirection = vNext - vCurrent;
					vDirection.normalise();
					Ray ray( vCurrent, vDirection );
					std::pair< bool, Real > intersect = ray.intersects( pl );

					// store intersection
					if ( intersect.first )
					{
						// convert distance to vector
						Vector3 vIntersect = ray.getPoint( intersect.second );

						// store intersection
						pNew->insertVertex( vIntersect );
						pIntersect->insertVertex( vIntersect );
					}

					pNew->insertVertex( vNext );

				}
				// else:
				// case 2: both outside (do nothing)
					
			}

			// insert the polygon only, if at least three vertices are present
			if ( pNew->getVertexCount() >= 3 )
			{
				// in case there are double vertices, remove them
				pNew->removeDuplicates();

				// in case there are still at least three vertices, insert the polygon
				if ( pNew->getVertexCount() >= 3 )
				{
					this->insertPolygon( pNew );
				}
				else
				{
					// delete pNew because it's empty or invalid
					freePolygon(pNew);
					pNew = 0;
				}
			}
			else
			{
				// delete pNew because it's empty or invalid
				freePolygon(pNew);
				pNew = 0;
			}

			// insert intersection polygon only, if there are two vertices present
			if ( pIntersect->getVertexCount() == 2 )
			{
				intersectionEdges.insert( Polygon::Edge( pIntersect->getVertex( 0 ),
														  pIntersect->getVertex( 1 ) ) );
			}

			// delete intersection polygon
			// vertices were copied (if there were any)
			freePolygon(pIntersect);
			pIntersect = 0;

			// delete side info
			OGRE_FREE(side, MEMCATEGORY_SCENE_CONTROL);
			side = 0;
		}

		// if the polygon was partially clipped, close it
		// at least three edges are needed for a polygon
		if ( intersectionEdges.size() >= 3 )
		{
			Polygon *pClosing = allocatePolygon();

			// Analyze the intersection list and insert the intersection points in ccw order
			// Each point is twice in the list because of the fact that we have a convex body
			// with convex polygons. All we have to do is order the edges (an even-odd pair)
			// in a ccw order. The plane normal shows us the direction.
			Polygon::EdgeMap::iterator it = intersectionEdges.begin();

			// check the cross product of the first two edges
			Vector3 vFirst  = it->first;
			Vector3 vSecond = it->second;

			// remove inserted edge
			intersectionEdges.erase( it );

			Vector3 vNext;

			// find mating edge
			if (findAndEraseEdgePair(vSecond, intersectionEdges, vNext))
			{
				// detect the orientation
				// the polygon must have the same normal direction as the plane and then n
				Vector3 vCross = ( vFirst - vSecond ).crossProduct( vNext - vSecond );
				bool frontside = ( pl.normal ).directionEquals( vCross, Degree( 1 ) );

				// first inserted vertex
				Vector3 firstVertex;
				// currently inserted vertex
				Vector3 currentVertex;
				// direction equals -> front side (walk ccw)
				if ( frontside )
				{
					// start with next as first vertex, then second, then first and continue with first to walk ccw
					pClosing->insertVertex( vNext );
					pClosing->insertVertex( vSecond );
					pClosing->insertVertex( vFirst );
					firstVertex		= vNext;
					currentVertex	= vFirst;

				#ifdef _DEBUG_INTERSECTION_LIST
					std::cout << "Plane: n=" << pl.normal << ", d=" << pl.d << std::endl;
					std::cout << "First inserted vertex: " << *next << std::endl;
					std::cout << "Second inserted vertex: " << *vSecond << std::endl;
					std::cout << "Third inserted vertex: " << *vFirst << std::endl;
				#endif
				}
				// direction does not equal -> back side (walk cw)
				else
				{
					// start with first as first vertex, then second, then next and continue with next to walk ccw
					pClosing->insertVertex( vFirst );
					pClosing->insertVertex( vSecond );
					pClosing->insertVertex( vNext );
					firstVertex		= vFirst;
					currentVertex	= vNext;

					#ifdef _DEBUG_INTERSECTION_LIST
						std::cout << "Plane: n=" << pl.normal << ", d=" << pl.d << std::endl;
						std::cout << "First inserted vertex: " << *vFirst << std::endl;
						std::cout << "Second inserted vertex: " << *vSecond << std::endl;
						std::cout << "Third inserted vertex: " << *next << std::endl;
					#endif
				}

				// search mating edges that have a point in common
				// continue this operation as long as edges are present
				while ( !intersectionEdges.empty() )
				{

					if (findAndEraseEdgePair(currentVertex, intersectionEdges, vNext))
					{
						// insert only if it's not the last (which equals the first) vertex
						if ( !intersectionEdges.empty() )
						{
							currentVertex = vNext;
							pClosing->insertVertex( vNext );
						}
					}
					else
					{
						// degenerated...
						break;
					}

				} // while intersectionEdges not empty

				// insert polygon (may be degenerated!)
				this->insertPolygon( pClosing );

			}
			// mating intersection edge NOT found!
			else
			{
				freePolygon(pClosing);
			}

		} // if intersectionEdges contains more than three elements
	}
	//-----------------------------------------------------------------------
	bool ConvexBody::findAndEraseEdgePair(const Vector3& vec, 
		Polygon::EdgeMap& intersectionEdges, Vector3& vNext ) const
	{
		for (Polygon::EdgeMap::iterator it = intersectionEdges.begin(); 
			it != intersectionEdges.end(); ++it)
		{
			if (it->first.positionEquals(vec))
			{
				vNext = it->second;

				// erase found edge
				intersectionEdges.erase( it );

				return true; // found!
			}
			else if (it->second.positionEquals(vec))
			{
				vNext = it->first;

				// erase found edge
				intersectionEdges.erase( it );

				return true; // found!
			}
		}

		return false; // not found!
	}
	//-----------------------------------------------------------------------
	void ConvexBody::logInfo( void ) const
	{
		StringUtil::StrStreamType ssOut( std::stringstream::out );
		ssOut << *this;
		
		Ogre::LogManager::getSingleton().logMessage( Ogre::LML_NORMAL, ssOut.str()  );
	}
}

